#include "Ultrasonic.h"

Ultrasonic::Ultrasonic(int trigPin, int echoPin) {
    TRIG_PIN = trigPin;
    ECHO_PIN = echoPin;
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
}

float Ultrasonic::getDistance() {
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    long duration = pulseIn(ECHO_PIN, HIGH, MAX_DISTANCE * 58);
    float distance = duration * SOUND_SPEED / 2;
    return distance;
}

bool Ultrasonic::checkObstacle(float threshold) {
    float distance = getDistance();

    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    return distance < threshold;
}
